//-------------------------------------------------
// Manipulator.cpp
//
//-------------------------------------------------


#include "dxstdafx.h"
#include "Manipulator.h"




CManipulator::CManipulator(void)
{
}

CManipulator::~CManipulator(void)
{
}

HRESULT CManipulator::Create(LPDIRECT3DDEVICE9 pd3dDevice)
{
	int i;
	cprimitive* primitive;
	
	//-----
	//manually create field parameters

	quadDimensions = D3DXVECTOR3(40, 40, 40);
	fieldDimensions = D3DXVECTOR2(10,10);

	//-----
	//manually create hierarchy

	//1st
	primitive = new cprimitive;
	cube.push_back(primitive);
	if(FAILED( cube[0]->Create(pd3dDevice, 400, D3DXVECTOR2(0, 0), 50.f) )) HTMLLog("cube[0] failed");

	//2nd
	primitive = new cprimitive;
	cube.push_back(primitive);
	if(FAILED( cube[1]->Create(pd3dDevice, 200, D3DXVECTOR2(D3DX_PI*0.2f,0), 40.f) )) HTMLLog("cube[1] failed");

	//3rd
	primitive = new cprimitive;
	cube.push_back(primitive);
	if(FAILED( cube[2]->Create(pd3dDevice, 300, D3DXVECTOR2(D3DX_PI*0.2f,0), 30.f) )) HTMLLog("cube[2] failed");

	//4th
	primitive = new cprimitive;
	cube.push_back(primitive);
	if(FAILED( cube[3]->Create(pd3dDevice, 500, D3DXVECTOR2(D3DX_PI*0.2f,0), 30.f) )) HTMLLog("cube[3] failed");

	//5th
	primitive = new cprimitive;
	cube.push_back(primitive);
	if(FAILED( cube[4]->Create(pd3dDevice, 500, D3DXVECTOR2(D3DX_PI*0.1f,0), 20.f) )) HTMLLog("cube[4] failed");

	//building dependencies
	cube[0]->pFirstChild = cube[1];
	cube[1]->pFirstChild = cube[2];
	cube[2]->pFirstChild = cube[3];
	cube[3]->pFirstChild = cube[4];


	//-----
	InitPredefinedPositions();


	return S_OK;
}

HRESULT CManipulator::SetAngles(int index, D3DXVECTOR2 angles)
{
	cube[index]->vAngle = angles;

	return S_OK;
}

D3DXVECTOR3 CManipulator::MoveTo(int quadrant, int level)
{
	//calculate absolute position of point
	float x = quadrant%(int)fieldDimensions.x * quadDimensions.x + quadDimensions.x*0.5f - fieldDimensions.x*0.5*quadDimensions.x;
	float y = (int)(quadrant/fieldDimensions.x) * quadDimensions.y + quadDimensions.y*0.5f;
	float z = level*quadDimensions.z;

	vDest = D3DXVECTOR3(x,y,z);
	InversedKinematics(vDest);

	return vDest;
}
HRESULT CManipulator::InversedKinematics(D3DXVECTOR3 vecDest)
{
	//rotating first cube
	SetAngles(0, D3DXVECTOR2(0, atan2(vecDest.x,vecDest.y)));

	return S_OK;
}

HRESULT CManipulator::FrameMove(float fElapsedTime, float fTime)
{
	//cube[0]->vAngle = D3DXVECTOR2( cube[0]->vAngle.x, cube[0]->vAngle.y + fElapsedTime);
	//cube[1]->vAngle = D3DXVECTOR2( D3DX_PI*0.2f + cos(fTime + 1), cube[1]->vAngle.y);
	//cube[2]->vAngle = D3DXVECTOR2( D3DX_PI*0.2f + cos(fTime + 2), cube[2]->vAngle.y);
	//cube[3]->vAngle = D3DXVECTOR2( D3DX_PI*0.2f + cos(fTime + 3), cube[3]->vAngle.y);
	//cube[4]->vAngle = D3DXVECTOR2( D3DX_PI*0.1f + cos(fTime + 4), cube[4]->vAngle.y);
	return S_OK;
}

HRESULT CManipulator::Render(LPDIRECT3DDEVICE9 pd3dDevice)
{
	int i;

	for(i=0; i<cube.size(); i++)
	{
		DrawChain(cube[0], pd3dDevice);
	}

	return S_OK;
}

HRESULT CManipulator::DrawChain(cprimitive* pCube, LPDIRECT3DDEVICE9 pd3dDevice)
{
	D3DXMATRIX matRotRes;
	D3DXMatrixIdentity(&matRotRes);

	DrawChain(pCube, pd3dDevice, matRotRes, matRotRes, D3DXVECTOR3(0,0,0));
	return S_OK;
}

HRESULT CManipulator::DrawChain(cprimitive* pCube, LPDIRECT3DDEVICE9 pd3dDevice, D3DXMATRIX matRot, D3DXMATRIX matRotY, D3DXVECTOR3 vecTranslate)
{
	D3DXMATRIX matRotRes, matRotYRes;
	D3DXVECTOR3 vecTransRes;
	matRotRes = matRot * pCube->GetRotationMatrix();
	matRotYRes = matRotY * pCube->GetRotationYMatrix();
	vecTransRes = vecTranslate;

	//HTMLLog("%d %d %d\n", (int)vecTransRes.x, (int)vecTransRes.y, (int)vecTransRes.z);
	//rendering
	D3DXMATRIX matTrans;
	D3DXMatrixTranslation(&matTrans, vecTransRes.x, vecTransRes.y, vecTransRes.z);
	D3DXMATRIX worldMatrix = matRotRes*matTrans*matRotYRes;
	pd3dDevice->SetTransform(D3DTS_WORLD, &worldMatrix);

	pCube->Render(pd3dDevice);
	
	if(pCube->pSiblings != NULL) DrawChain(pCube->pSiblings, pd3dDevice, matRot, matRotY, vecTranslate);
	if(pCube->pFirstChild != NULL) DrawChain(pCube->pFirstChild, pd3dDevice, matRotRes, matRotYRes, vecTransRes + pCube->GetTranslationVector(matRotRes));


	return S_OK;
}

HRESULT CManipulator::Release()
{
	int i;

	for(i=0; i<cube.size(); i++) 
		cube[i]->Release();

	return S_OK;
}

HRESULT CManipulator::InitPredefinedPositions()
{
	//manually setup predefined positions
	numPredefinedPos = 2;

	//1st - straight stick
	D3DXVECTOR2* angles;
	angles = new D3DXVECTOR2[4];
	angles[0] = D3DXVECTOR2(0,0);
	angles[1] = D3DXVECTOR2(0,0);
	angles[2] = D3DXVECTOR2(0,0);
	angles[3] = D3DXVECTOR2(0,0);
	angles[4] = D3DXVECTOR2(0,0);
	
	predefinedPositions.push_back(angles);

	//2nd - lightly bended
	angles = new D3DXVECTOR2[4];
	angles[0] = D3DXVECTOR2(0,0);
	angles[1] = D3DXVECTOR2(D3DX_PI*0.2f,0);
	angles[2] = D3DXVECTOR2(D3DX_PI*0.2f,0);
	angles[3] = D3DXVECTOR2(D3DX_PI*0.2f,0);
	angles[4] = D3DXVECTOR2(D3DX_PI*0.2f,0);

	predefinedPositions.push_back(angles);
	return S_OK;
}

HRESULT CManipulator::PredefinedPosition(int num)
{
	int i;
	if(num >= numPredefinedPos) return E_FAIL;

	for( i = 0; i < 5; i++ )
	{
		SetAngles(i, predefinedPositions[num][i]);
	}
	return S_OK;
}